'''
@file:    find_red_follow.py
@company: Hiwonder
@author:  CuZn
@date:    2023-10-10
@description: 红色识别跟随
'''

from hiwonder import hw_uart
import sensor
import image
import time
import lcd
import binascii
#import numpy as np

class Value_Data:
    def __init__(self):
        self.img_width = 320
        self.img_height = 240
        self.point_y_start = round(self.img_height/5)
        self.point_x_start = round(self.img_width/5)

data = Value_Data()
print("data.img_width:", data.point_y_start)

#定义串口对象
serial = hw_uart()

##################################### send_func begin #####################################

#CRC校验参数，不可修改，否则数据通讯会出错
crc8_table = [
    0, 94, 188, 226, 97, 63, 221, 131, 194, 156, 126, 32, 163, 253, 31, 65,
    157, 195, 33, 127, 252, 162, 64, 30, 95, 1, 227, 189, 62, 96, 130, 220,
    35, 125, 159, 193, 66, 28, 254, 160, 225, 191, 93, 3, 128, 222, 60, 98,
    190, 224, 2, 92, 223, 129, 99, 61, 124, 34, 192, 158, 29, 67, 161, 255,
    70, 24, 250, 164, 39, 121, 155, 197, 132, 218, 56, 102, 229, 187, 89, 7,
    219, 133, 103, 57, 186, 228, 6, 88, 25, 71, 165, 251, 120, 38, 196, 154,
    101, 59, 217, 135, 4, 90, 184, 230, 167, 249, 27, 69, 198, 152, 122, 36,
    248, 166, 68, 26, 153, 199, 37, 123, 58, 100, 134, 216, 91, 5, 231, 185,
    140, 210, 48, 110, 237, 179, 81, 15, 78, 16, 242, 172, 47, 113, 147, 205,
    17, 79, 173, 243, 112, 46, 204, 146, 211, 141, 111, 49, 178, 236, 14, 80,
    175, 241, 19, 77, 206, 144, 114, 44, 109, 51, 209, 143, 12, 82, 176, 238,
    50, 108, 142, 208, 83, 13, 239, 177, 240, 174, 76, 18, 145, 207, 45, 115,
    202, 148, 118, 40, 171, 245, 23, 73, 8, 86, 180, 234, 105, 55, 213, 139,
    87, 9, 235, 181, 54, 104, 138, 212, 149, 203, 41, 119, 244, 170, 72, 22,
    233, 183, 85, 11, 136, 214, 52, 106, 43, 117, 151, 201, 74, 20, 246, 168,
    116, 42, 200, 150, 21, 75, 169, 247, 182, 232, 10, 84, 215, 137, 107, 53
]

#CRC校验函数
def checksum_crc8(data):
    # 校验
    check = 0
    for b in data:
        check = crc8_table[check ^ b]
    return check & 0x00FF

#功能号
class FuncTag:
    START = 0
    FIND_RED_FOLLOW  = 0x0C
    FIND_FIELD_FOLLOW = 0x10


#字符串转int
def str_2_int(data_str):
    bb = binascii.hexlify(data_str)
    bb = str(bb)[2:-1]
    int_h = int(bb[0])*16
    int_l = int(bb[1],16)
    return int_h+int_l


def send_data(x,y,w,h,msg):
    '''
    0xAA  0x55  功能号  数据长度  data  CRC
    '''
    START_1 = 0xAA
    START_2 = 0x55
    FUNC_num = FuncTag.FIND_RED_FOLLOW #功能编号
    Length = 0  #数据长度
    crc = 0 #校验位
    data = [] #数据组

    #参数都为0
    if x==0 and y==0 and w==0 and h ==0:
        pass
    else:
        #x(小端模式)
        low = x & 0xFF #低位
        high = x >> 8& 0xFF #高位
        data.append(low)
        data.append(high)

        #y(小端模式)
        low = y & 0xFF #低位
        high = y >> 8& 0xFF #高位
        data.append(low)
        data.append(high)

        #w(小端模式)
        low = w & 0xFF #低位
        high = w >> 8& 0xFF #高位
        data.append(low)
        data.append(high)

        #h(小端模式)
        low = h & 0xFF #低位
        high = h >> 8& 0xFF #高位
        data.append(low)
        data.append(high)

    #msg
    if msg != None:
        for i in range(len(msg)):
            msg_int = str_2_int(msg[i])
            data.append(msg_int)

    Length += len(data)
    send_buf = [FUNC_num,Length]
    for i in range(len(data)):
        send_buf.append(data[i])

    #进行CRC运算
    crc = checksum_crc8(send_buf)

    send_buf.insert(0,START_1) #插入协议头1  AA
    send_buf.insert(1,START_2) #插入协议头2  55
    send_buf.append(crc) #加入CRC校验码

    #print(send_buf) #打印数据
    serial.send_bytearray(send_buf) #发送数据


def send_data_field(x,y,w,h,msg):
    '''
    0xAA  0x55  功能号  数据长度  data  CRC
    '''
    START_1 = 0xAA
    START_2 = 0x55
    FUNC_num = FuncTag.FIND_FIELD_FOLLOW #功能编号
    Length = 0  #数据长度
    crc = 0 #校验位
    data = [] #数据组

    #参数都为0
    if x==0 and y==0 and w==0 and h ==0:
        pass
    else:
        #x(小端模式)
        low = x & 0xFF #低位
        high = x >> 8& 0xFF #高位
        data.append(low)
        data.append(high)

        #y(小端模式)
        low = y & 0xFF #低位
        high = y >> 8& 0xFF #高位
        data.append(low)
        data.append(high)

        #w(小端模式)
        low = w & 0xFF #低位
        high = w >> 8& 0xFF #高位
        data.append(low)
        data.append(high)

        #h(小端模式)
        low = h & 0xFF #低位
        high = h >> 8& 0xFF #高位
        data.append(low)
        data.append(high)

    #msg
    if msg != None:
        for i in range(len(msg)):
            msg_int = str_2_int(msg[i])
            data.append(msg_int)

    Length += len(data)
    send_buf = [FUNC_num,Length]
    for i in range(len(data)):
        send_buf.append(data[i])

    #进行CRC运算
    crc = checksum_crc8(send_buf)

    send_buf.insert(0,START_1) #插入协议头1  AA
    send_buf.insert(1,START_2) #插入协议头2  55
    send_buf.append(crc) #加入CRC校验码

    print(send_buf) #打印数据
    serial.send_bytearray(send_buf) #发送数据

##################################### send_func end #####################################

#定义发送数据
send_x = 0
send_y = 0
send_w = 0
send_h = 0



#初始化LCD
lcd.init()
#以下是初始化传感器
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.skip_frames(time = 100)
sensor.set_auto_gain(False)
sensor.set_auto_whitebal(False)
#帧率时钟
clock = time.clock()

#方框参数
r = [(320//2)-(50//2), (240//2)-(50//2), 50, 50]


# 储存红色的LAB阈值
color_thresholds = [
    #(20,80,20,62,20,35), #Red
    #(38,79,-14,-2,18,39), #Pale yellow
    #(30, 34, 40, 48, 17, 29), #RED color bool
    #(28, 72, -6, 6, -21, -6), #BLUE color bool
    #(31, 61, -2, 17, -33, -13),#darkness blue color bool

    (24, 66, -27, -9, 0, 22),
]

color_field_thresholds = [
    #(45, 51, 40, 53, 18, 33), #red
    #(44, 67, -6, 7, -40, -27), #Pale blue
    #(24, 36, 36, 55, 14, 37), #Pale RED
    #(28, 31, 35, 44, 8, 21),
    #(63, 81, -22, -2, -18, 0),
    #(55, 70, -19, -8, -22, -11),
    #(32, 63, -17, -1, -27, -2),

    (43, 51, -36, -26, 4, 19),
]

print("Start Color Recognition...")


#loop
while True:
    point_y_start = 48
    #用于计算帧率的函数，这里表示开始计时
    clock.tick()
    #从传感器捕获一张图像
    img = sensor.snapshot()
    fps = clock.fps()
    #===================================================================================================================================
    #遍历多种颜色
    for color_idx, threshold in enumerate(color_thresholds):
        #遍历图像中找到的颜色区块
        blobs = img.find_blobs([threshold], pixels_threshold=100, area_threshold=100, merge=True, margin=10)
        #是否找到
        if blobs:
            area_max = 0
            blob_max = blobs[0]
            #寻找最大的颜色块
            for blob in blobs:
                area = blob.w()*blob.h()
                if(area_max < area):
                    blob_max = blob
                    area_max = area
            if area_max < 200: # 限制识别到的方框大小
                print("big")
                continue
            #画方框

            # 起点和终点的坐标
            #img.draw_line((img_height/5)*0, round(img_height/5)*1, img_width, round(img_height/5)*1, color=(210, 105, 30), thickness=3)
            img.draw_line(0, data.point_y_start*1, data.img_width, data.point_y_start*1, color=(210, 105, 30), thickness=3)
            img.draw_line(0, data.point_y_start*2, data.img_width, data.point_y_start*2, color=(210, 105, 30), thickness=3)
            img.draw_line(0, data.point_y_start*3, data.img_width, data.point_y_start*3, color=(210, 105, 30), thickness=3)
            img.draw_line(0, data.point_y_start*4, data.img_width, data.point_y_start*4, color=(210, 105, 30), thickness=3)

            img.draw_line(data.point_x_start*1, 0, data.point_x_start*1, data.img_height, color=(210, 105, 30), thickness=3)
            img.draw_line(data.point_x_start*2, 0, data.point_x_start*2, data.img_height, color=(210, 105, 30), thickness=3)
            img.draw_line(data.point_x_start*3, 0, data.point_x_start*3, data.img_height, color=(210, 105, 30), thickness=3)
            img.draw_line(data.point_x_start*4, 0, data.point_x_start*4, data.img_height, color=(210, 105, 30), thickness=3)

            img.draw_rectangle(blob_max.rect())
            img.draw_cross(blob_max.cx(), blob_max.cy())
            #img.draw_string(blob_max.cx() + 10, blob_max.cy() - 10, 'sth', color=(255, 255, 255))
            #将方框数据与消息赋值
            send_x = blob_max.x()
            send_y = blob_max.y()
            send_w = blob_max.w()
            send_h = blob_max.h()
            img.draw_string(blob_max.cx() - 20, blob_max.cy() - 10, str(send_x), color=(255, 255, 255))
            img.draw_string(blob_max.cx() + 20, blob_max.cy() - 10, str(send_y), color=(255, 255, 255))
            #发送数据
            send_data(send_x,send_y,send_w,send_h,None)
    #img.draw_string(0, 0, "%2.1ffps" %(fps), color=(0, 60, 255), scale=2.0)
    #显示在LCD上

    #===================================================================================================================================
    for color_idx, threshold in enumerate(color_field_thresholds):
        #遍历图像中找到的颜色区块
        blobs = img.find_blobs([threshold], pixels_threshold=100, area_threshold=100, merge=True, margin=10)
        #是否找到
        if blobs:
            area_max = 0
            blob_max = blobs[0]
            #寻找最大的颜色块
            for blob in blobs:
                area = blob.w()*blob.h()
                if(area_max < area):
                    blob_max = blob
                    area_max = area
            if area_max < 200: # 限制识别到的方框大小
                print("big")
                continue
            #画方框

            # 起点和终点的坐标
            #img.draw_line((img_height/5)*0, round(img_height/5)*1, img_width, round(img_height/5)*1, color=(210, 105, 30), thickness=3)
            #img.draw_line(0, data.point_y_start*1, data.img_width, data.point_y_start*1, color=(210, 105, 30), thickness=3)
            #img.draw_line(0, data.point_y_start*2, data.img_width, data.point_y_start*2, color=(210, 105, 30), thickness=3)
            #img.draw_line(0, data.point_y_start*3, data.img_width, data.point_y_start*3, color=(210, 105, 30), thickness=3)
            #img.draw_line(0, data.point_y_start*4, data.img_width, data.point_y_start*4, color=(210, 105, 30), thickness=3)

            #img.draw_line(data.point_x_start*1, 0, data.point_x_start*1, data.img_height, color=(210, 105, 30), thickness=3)
            #img.draw_line(data.point_x_start*2, 0, data.point_x_start*2, data.img_height, color=(210, 105, 30), thickness=3)
            #img.draw_line(data.point_x_start*3, 0, data.point_x_start*3, data.img_height, color=(210, 105, 30), thickness=3)
            #img.draw_line(data.point_x_start*4, 0, data.point_x_start*4, data.img_height, color=(210, 105, 30), thickness=3)

            img.draw_rectangle(blob_max.rect())
            img.draw_cross(blob_max.cx(), blob_max.cy())
            #img.draw_string(blob_max.cx() + 10, blob_max.cy() - 10, 'field', color=(0, 0, 255))
            #将方框数据与消息赋值
            send_x = blob_max.x()
            send_y = blob_max.y()
            send_w = blob_max.w()
            send_h = blob_max.h()
            img.draw_string(blob_max.cx() - 20, blob_max.cy() - 10, str(send_x), color=(255, 255, 255))
            img.draw_string(blob_max.cx() + 20, blob_max.cy() - 10, str(send_y), color=(255, 255, 255))
            #发送数据
            send_data_field(send_x,send_y,send_w,send_h,None)
    #img.draw_string(0, 0, "%2.1ffps" %(fps), color=(0, 60, 255), scale=2.0)
    #显示在LCD上



    lcd.display(img)
    #打印帧率
    #print(fps)
